#include "scan_filter.hpp"

namespace AgvObstaAvoid
{

ScanFilterChain::ScanFilterChain() 
{
    // Initialize parameters with default values
    filter_type_ = 1;
    speckle_max_range_ = 5.0;
    max_range_difference_ = 0.2;
    filter_window_ = 4;

    min_angle_shadows_ = 5.0;
    max_angle_shadows_ = 175.0;
    window_shadows_ = 2;
    neighbors_shadows_ = 3;
    remove_shadow_start_point_shadows_ = false;
    angle_increment_shadows_ = 0.0;

    filter_enable = true;
    speckleFilter_enable_ = false;
    shadowsFilter_enable_ = false;

    // Create publishers
    // scan_filter_1_ = this->create_publisher<sensor_msgs::msg::LaserScan>("scan_filtered_1", 10);
    // scan_filter_2_ = this->create_publisher<sensor_msgs::msg::LaserScan>("scan_filtered_2", 10);

    RCLCPP_INFO(rclcpp::get_logger("ScanFilterChain"), "Scan filter node initialized");
}

bool ScanFilterChain::FilterReadXml(tinyxml2::XMLElement* element)
{
    RCLCPP_INFO(rclcpp::get_logger("ScanFilterChain"), "Initializing lidar filter xml parameters");
    tinyxml2::XMLElement* type_node = element->FirstChildElement();

    try {
        while (type_node) {
            if (std::string(type_node->Attribute("name")) == "ShadowsFilter") {
                shadowsFilter_enable_ = type_node->Attribute("enable") ? 
                    std::stoi(std::string(type_node->Attribute("enable"))) : false;
                max_angle_shadows_ = type_node->Attribute("max_angle") ? 
                    std::stoi(std::string(type_node->Attribute("max_angle"))) : 175;
                min_angle_shadows_ = type_node->Attribute("min_angle") ? 
                    std::stoi(std::string(type_node->Attribute("min_angle"))) : 5;
                neighbors_shadows_ = type_node->Attribute("neighbors") ? 
                    std::stoi(std::string(type_node->Attribute("neighbors"))) : 3;
                window_shadows_ = type_node->Attribute("windows") ? 
                    std::stoi(std::string(type_node->Attribute("windows"))) : 2;
                
                remove_shadow_start_point_shadows_ = false;
                angle_increment_shadows_ = 0;

                min_angle_tan_ = tanf(angles::from_degrees(min_angle_shadows_));
                max_angle_tan_ = tanf(angles::from_degrees(max_angle_shadows_));
                if (min_angle_tan_ < 0.0) min_angle_tan_ = -min_angle_tan_;
                if (max_angle_tan_ > 0.0) max_angle_tan_ = -max_angle_tan_;
            }

            if (std::string(type_node->Attribute("name")) == "SpeckleFilter") {
                speckleFilter_enable_ = type_node->Attribute("enable") ? 
                    std::stoi(std::string(type_node->Attribute("enable"))) : false;
                filter_type_ = type_node->Attribute("filter_type") ? 
                    std::stoi(std::string(type_node->Attribute("filter_type"))) : 1;
                max_range_difference_ = type_node->Attribute("max_range_difference") ? 
                    atof(type_node->Attribute("max_range_difference")) : 0.2;
                filter_window_ = type_node->Attribute("neighbors") ? 
                    std::stoi(std::string(type_node->Attribute("neighbors"))) : 4;
                speckle_max_range_ = type_node->Attribute("max_range") ? 
                    std::stoi(std::string(type_node->Attribute("max_range"))) : 5;
            }
            type_node = type_node->NextSiblingElement();
        }
    } catch(const std::exception& e) {
        RCLCPP_ERROR(rclcpp::get_logger("ScanFilterChain"), 
            "Error reading lidar filter xml parameters: %s", e.what());
        return false;
    }

    RCLCPP_INFO(rclcpp::get_logger("ScanFilterChain"), "Filter parameters:");
    RCLCPP_INFO(rclcpp::get_logger("ScanFilterChain"), 
        "ShadowsFilter: enable=%d, max_angle=%.1f, min_angle=%.1f, neighbors=%d, window=%d",
                shadowsFilter_enable_, max_angle_shadows_, min_angle_shadows_, neighbors_shadows_, window_shadows_);
    RCLCPP_INFO(rclcpp::get_logger("ScanFilterChain"), 
        "SpeckleFilter: enable=%d, filter_type=%d, max_range_difference=%.2f, neighbors=%d, max_range=%.1f",
                speckleFilter_enable_, filter_type_, max_range_difference_, filter_window_, speckle_max_range_);

    return true;
}

void ScanFilterChain::LidarSubCallback(const sensor_msgs::msg::LaserScan::SharedPtr scan_msg, int equip_name)
{
    sensor_msgs::msg::LaserScan msg_;
    
    if (filter_enable) {
        ShadowsFilter(*scan_msg, msg_);
    } else {
        msg_ = *scan_msg;
    }
    
    if (equip_name == 1) {
        scan_filter_1_->publish(msg_);
    } else if (equip_name == 2) {
        scan_filter_2_->publish(msg_);
    }
}

bool ScanFilterChain::ShadowsFilter(const sensor_msgs::msg::LaserScan& scan_in, sensor_msgs::msg::LaserScan& scan_out)
{
    std::lock_guard<std::mutex> lock(own_mutex_);
    int count_filted = 0;
    int count = 0;
    
    // copy across all data first
    scan_out = scan_in;

    int size = scan_in.ranges.size();
    int max_y;
    int max_neighbors;
    ShadowsPrepareForInput(scan_in.angle_increment);
    
    // For each point in the current line scan
    for (int i = 0; i < size; i++) {
        max_y = std::min<int>(size - i, window_shadows_ + 1);
        for (int y = std::max<int>(-i, -window_shadows_); y < max_y; y++) {
            if (y == 0) continue;
          
            if (ShadowsisShadow(scan_in.ranges[i], scan_in.ranges[i + y], 
                                sin_map_shadows_[y + window_shadows_], cos_map_shadows_[y + window_shadows_])) {
                max_neighbors = std::min<int>(i + neighbors_shadows_, size - 1);
                for (int index = std::max<int>(i - neighbors_shadows_, 0); index <= max_neighbors; index++) {
                    if (scan_in.ranges[i] < scan_in.ranges[index]) {  // delete neighbor if they are farther away (note not self)
                        scan_out.ranges[index] = std::numeric_limits<float>::quiet_NaN();
                        count_filted++; 
                    }
                }
                if (remove_shadow_start_point_shadows_) {
                    scan_out.ranges[i] = std::numeric_limits<float>::quiet_NaN(); 
                }
                break;
            }
        }
    }

    sensor_msgs::msg::LaserScan filtered_scan = scan_out;
    std::vector<bool> &valid_ranges = valid_ranges_work_;

    // Check if range size is big enough to use the filter window
    if (scan_out.ranges.size() <= filter_window_ + 1){
        RCLCPP_ERROR(rclcpp::get_logger("ScanFilterChain"), 
            "Scan ranges size is too small: size = %ld", scan_out.ranges.size());
        return false;
    }

    size_t i = 0;
    size_t i_max = filtered_scan.ranges.size();
    valid_ranges.clear();
    while (i < i_max) {
        // Check range data against speckle_max_range_
        bool out_of_range = scan_out.ranges[i] > speckle_max_range_;
        valid_ranges.push_back(out_of_range);
        ++i;
    }

    // Control loop for filtering operation
    i = 0;
    i_max = filtered_scan.ranges.size() - filter_window_ + 1;
    while (i < i_max) {
        bool window_valid = validator_.checkWindowValid(scan_out, i, filter_window_, max_range_difference_);
        if (window_valid) {
            size_t j = i, j_max = i + filter_window_;
            do {
                valid_ranges[j++] = true;
            } while (j < j_max);
        }
        ++i;
    }

    i = 0;
    i_max = valid_ranges.size();
    while (i < i_max) {
        if (!valid_ranges[i]) {
            scan_out.ranges[i] = std::numeric_limits<float>::quiet_NaN();
            ++count;
        }
        ++i;
    }

    return true;
}

void ScanFilterChain::ShadowsPrepareForInput(const float angle_increment)
{
    if (angle_increment_shadows_ != angle_increment) {
        RCLCPP_DEBUG(rclcpp::get_logger("ScanFilterChain"), "No precomputed map given. Computing one.");
        angle_increment_shadows_ = angle_increment;
        sin_map_shadows_.clear();
        cos_map_shadows_.clear();

        float included_angle = -window_shadows_ * angle_increment;

        for (int i = -window_shadows_; i <= window_shadows_; ++i) {
            sin_map_shadows_.push_back(fabs(sinf(included_angle)));
            cos_map_shadows_.push_back(cosf(included_angle));
            included_angle += angle_increment;
        }
    }
}

bool ScanFilterChain::ShadowsisShadow(float r1, float r2, float included_angle_sin, float included_angle_cos)
{
    const float perpendicular_y_ = r2 * included_angle_sin;
    const float perpendicular_x_ = r1 - r2 * included_angle_cos;
    const float perpendicular_tan_ = fabs(perpendicular_y_) / perpendicular_x_;

    if (perpendicular_tan_ > 0) {
        if (perpendicular_tan_ < min_angle_tan_)
            return true;
    }
    else {
        if (perpendicular_tan_ > max_angle_tan_)
            return true;
    }
    return false;
}

} // namespace AgvObstaAvoid